import threading

from Control import Keyboard

output_mod = False


class Mission:
    keyboard = Keyboard.Keyboard()

    control = None

    mission_numb = 0
    control_switch = 1

    send_switch = False

    on_fly = False

    speed = 40
    roll = 0
    pitch = 0
    throttle = 0
    yaw = 0

    main_thread = None
    send_thread = None
    switch_thread = None

    def __init__(self, control=None):
        self.control = control

    def start(self):
        self.keyboard.start()

        if self.main_thread is None:
            self.main_thread = threading.Thread(target=self.maintain)
        self.main_thread.start()

        if self.switch_thread is None:
            self.switch_thread = threading.Thread(target=self.switch)
        self.switch_thread.start()

        if self.send_thread is None:
            self.send_thread = threading.Thread(target=self.send)
        self.send_thread.start()

    def maintain(self):
        while True:
            self.control_deal()
            self.mission_deal()

    def switch(self):
        while True:
            if self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'p':
                self.control_switch = 0
                if self.on_fly:
                    self.send_switch = True
                else:
                    self.send_switch = False
            if self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'o':
                self.control_switch = 1
                self.send_switch = False

    def send(self):
        while True:
            if self.send_switch and self.on_fly and self.control_switch == 1:
                self.control.set_rc(self.roll, self.pitch, self.throttle, self.yaw)

    def control_deal(self):
        while self.control_switch == 1:
            # 发送手动控制信息
            if self.on_fly:
                if self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'w':
                    self.pitch = -self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 's':
                    self.pitch = self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'w':
                    self.pitch = 0
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 's':
                    self.pitch = 0

                if self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'a':
                    self.roll = -self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'd':
                    self.roll = self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'a':
                    self.roll = 0
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'd':
                    self.roll = 0

                if self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'shift':
                    self.throttle = -self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'space':
                    self.throttle = self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'shift':
                    self.throttle = 0
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'space':
                    self.throttle = 0

                if self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'left':
                    self.yaw = -self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'down' and self.keyboard.event.name == 'right':
                    self.yaw = self.speed
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'left':
                    self.yaw = 0
                elif self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'right':
                    self.yaw = 0

                if self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 'y':
                    self.on_fly = False
                    self.control.land()
                    self.control.wait()

            else:
                if self.keyboard.event is not None and self.keyboard.event.event_type == 'up' and self.keyboard.event.name == 't':
                    self.control.takeoff()
                    self.control.wait()
                    self.on_fly = True
                    if output_mod:
                        print("keyboard: takeoff")

    def mission_deal(self):
        while self.control_switch == 0:
            if self.mission_numb == 0:
                pass
            elif self.mission_numb == 1:
                pass


if __name__ == '__main__':
    m = Mission()
    while True:
        print(6)
